﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Reflection;

namespace Visualization.DataModel
{
    public class SFPStep
    {
        /// <summary>
        /// Step number - Each step is .25 milliseconds
        /// </summary>
        public int Step { get; set; }
        /// <summary>
        /// Reference Time from start of data logging
        /// </summary>
        public double RefTime { get; set; }
        /// <summary>
        /// Published X value
        /// </summary>
        public double X { get; set; }
        /// <summary>
        /// Published Y value
        /// </summary>
        public double Y { get; set; }
        /// <summary>
        /// Published theta value
        /// </summary>
        public double Theta { get; set; }
        /// <summary>
        /// Particles created for each step
        /// </summary>
        public List<Particle> particles { get; set; }
        /// <summary>
        /// Particle with max probability in the SFP Step
        /// </summary>
        public Particle HighestProbabilityParticle { get; set; }
        public double StandardDeviation, average, xSigma, xBar, ySigma, yBar, thetaSigma, thetaBar;

        public SFPStep(){
            HighestProbabilityParticle = new Particle(){Probability = 0.0};
        }

        public SFPStep(int StepNum, double ReferenceTime){
            this.Step = StepNum;
            this.RefTime = ReferenceTime;
            this.HighestProbabilityParticle = new Particle() { Probability = 0.0 };
        }

        public SFPStep(int StepNum, double ReferenceTime, double predictedX, double predictedY, double predictedTheta){
            this.Step = StepNum;
            this.RefTime = ReferenceTime;
            this.X = predictedX;
            this.Y = predictedY;
            this.Theta = predictedTheta;
            this.HighestProbabilityParticle = new Particle() { Probability = 0.0 };
        }

        /// <summary>
        /// Calculate the standard deviation of particles using normalization errors
        /// </summary>
        public void calcStandardDeviation() {
            //first calculate the standard deviation of the weights of each particle
            double VTwo = 0, weightSum = 0, xVar=0,yVar=0,thetaVar=0;
            double sumOfDerivationAverage = 0;
            particles.ForEach(delegate(Particle p) {
                average += p.Weight;
                sumOfDerivationAverage += Math.Pow(p.Weight, 2.0);
                //averages
                xBar += p.Weight * p.X;
                yBar += p.Weight * p.Y;
                thetaBar += p.Weight * p.Theta;
                //sum of derivation averages
                VTwo = p.Weight * p.Weight;
                weightSum += p.Weight;
            });
            average /= particles.Count;
            sumOfDerivationAverage /= particles.Count;
            xBar /= weightSum;
            yBar /= weightSum;
            thetaBar /= weightSum;
            particles.ForEach(delegate(Particle p){
                xVar += p.Weight * Math.Pow(p.X - xBar, 2.0);
                yVar += p.Weight * Math.Pow(p.Y - yBar, 2.0);
                thetaVar += p.Weight * Math.Pow(p.Theta - thetaBar, 2.0);
            });
            xSigma = Math.Sqrt((1 / (1 - VTwo)) * xVar);
            ySigma = Math.Sqrt((1 / (1 - VTwo)) * yVar);
            thetaSigma = Math.Sqrt((1 / (1 - VTwo)) * thetaVar);
            StandardDeviation = Math.Sqrt(sumOfDerivationAverage - Math.Pow(average, 2.0));
        }

        /// <summary>
        /// Generate probabilities for each particle based on standard deviation and stores particle with highest probability
        /// </summary>
        public void generateProbabilities(){
            particles.ForEach(delegate(Particle p){
                double zScore = (p.Weight - this.average) / this.StandardDeviation;
                double z = -Math.Pow(zScore, 2.0) / 2;
                p.Probability = (1 / Math.Sqrt(2 * Math.PI)) * (Math.Exp(zScore));
                if (p.Probability > HighestProbabilityParticle.Probability)
                    HighestProbabilityParticle = p;
            });
        }
    }
}
